package com.evolved.automata.android.bots.speechbot;
import com.evolved.automata.*;

import java.util.*;

public class RobotController 
{
	

	public static final float MAX_DIFFERENCE_FOR_EQUALITY = 2F; //  
	public static final int MAX_POWER=100;
	public static final int MIN_POWER=-100;
	

    /********************************************************
     * Begin of State Predicates 
     ********************************************************
     */
	
	Hashtable<String,String> generalState;
	Hashtable<String, Predicate> predicateMap;
	int leftMotorTach; // k
	int prevLeftMotorTach;
	
	int rightMotorTach; // k
	int prevRightMotorTach;
	
	int ultrasonic;
	int battery; // k
	boolean frontBumper;
	
	int leftMotorPower=50;
	int rightMotorPower=50;
	
	
	int prevLeftMotorPower=0;
	int prevRightMotorPower=0;
	
	int deltaTime;
	long lastTimeMilli;
	
	float averageMotorEfficiency;
	
	int loopCounter=0;
	
	float leftMotorEfficiency;
	float rightMotorEfficiency;

	
	int prevLeftMototDeltaTach;
	int prevRightMotorDeltaTach;
	
	
	DataDistribution leftEfficiencyStats;
	DataDistribution rightEfficiencyStats;
	
	int maxLeftStdBuffer=10;
	int maxRightStdBuffer=10;
	
	
	DataDistribution leftTachStats;
	DataDistribution rightTachStats;
	
	String topLevelFunction;
	
	InferenceController controller;
	
	private static KeyValuePair kvActionDefFilter = new KeyValuePair("action_defP","T");
	private static KeyValuePair kvExitConditionFilter = new KeyValuePair("exitP","T");
	private static String step_filter_key="step";
	private static String total_steps_filter_key="total_steps";
	private static String action_ref_filter_key="ref_action";
	private static KeyValuePair kvLastStepReachedPred = new KeyValuePair("last_step_reached","T");
	private static String function_filter_key="function";
	private static KeyValuePair kvSegmentExitFilterPair  = new KeyValuePair("conditions_P","T");
	private static KeyValuePair kvSegmentDefPair  = new KeyValuePair("segment_defP","T");
	Hashtable<String,String> predicateValues;
	
	
	public static class ActionResponse
	{
		public int leftPower;
		public int rightPower;
		
		public ActionResponse(int left, int right)
		{
			leftPower=left;
			rightPower=right;
		}
	}
	
	
	
	
	public RobotController()
	{
		generalState = new Hashtable<String, String>();
		predicateMap = new Hashtable<String, Predicate>();
		leftEfficiencyStats = new DataDistribution(maxLeftStdBuffer);
		rightEfficiencyStats = new DataDistribution(maxRightStdBuffer);
		predicateValues = new Hashtable<String, String>();
		createPredicates();
		controller = InferenceApp.controller;
	}
	
	
	public ActionResponse updateBotData(Hashtable<String,Integer> dataBlob, String speechData)
	{
		long currentTimeMilli=System.currentTimeMillis();
		deltaTime=(int)(currentTimeMilli-lastTimeMilli);
		lastTimeMilli=currentTimeMilli;
		prevRightMotorTach=rightMotorTach;
		prevLeftMotorTach=leftMotorTach;
		
		int leftMotorPower=0;
		int rightMotorPower=0;
		
		// Updated values
		leftMotorTach=dataBlob.get("leftTach");
		rightMotorTach=dataBlob.get("rightTach");
		battery=dataBlob.get("battery");
		frontBumper=dataBlob.get("switch").intValue()>500;
		ultrasonic=dataBlob.get("ultrasonic");
		
		// derived sensors
		if (prevRightMotorPower!=0)
		{
			rightMotorEfficiency=(rightMotorTach-prevRightMotorTach)/prevRightMotorPower/deltaTime;
			rightEfficiencyStats.add(rightMotorEfficiency);
		}
		
		if (prevLeftMotorPower!=0)
		{
			leftMotorEfficiency=(leftMotorTach-prevLeftMotorTach)/prevLeftMotorPower/deltaTime;
			leftEfficiencyStats.add(leftMotorEfficiency);
		}
		
		
		loopCounter++;
		
		// Update predicates
		updatePredicates();
		
		ActionResponse nextResponse;
		if (topLevelFunction!=null)
		{
			nextResponse=executeFunction(topLevelFunction,null);
			if (prevLeftMotorPower!=nextResponse.leftPower)
			{
				predicateMap.get("left_tach_1").setParameterIndex(leftMotorTach,2);
				predicateMap.get("left_tach_2").setParameterIndex(leftMotorTach,2);
				predicateMap.get("left_tach_3").setParameterIndex(leftMotorTach,2);
				predicateMap.get("left_tach_4").setParameterIndex(leftMotorTach,2);
			}
			
			if (prevRightMotorPower!=nextResponse.rightPower)
			{
				predicateMap.get("right_tach_1").setParameterIndex(rightMotorTach,2);
				predicateMap.get("right_tach_2").setParameterIndex(rightMotorTach,2);
				predicateMap.get("right_tach_3").setParameterIndex(rightMotorTach,2);
				predicateMap.get("right_tach_4").setParameterIndex(rightMotorTach,2);
			}
			
			prevLeftMotorPower=nextResponse.leftPower;
			prevRightMotorPower = nextResponse.rightPower;
			return nextResponse;
		}
		else
		{
			prevLeftMotorPower=0;
			prevRightMotorPower=0;
			return new ActionResponse(leftMotorPower,rightMotorPower);
		}


		
	}

	
	public void createPredicates()
	{
		// Add distance measure
		addPredicate("front_collision",
				new Predicate()
				{
					public boolean truthValue()
					{
						return frontBumper;
					}
				});
		
		addPredicate("invalid_ultra",
				new Predicate()
				{
					public boolean truthValue()
					{
						return ultrasonic==255;
					}
				});
		
		addPredicate("ultra_greater_40",
				new Predicate(new Integer(40))
				{
					public boolean truthValue()
					{
						Integer compValue = (Integer)param1;
						return ultrasonic>compValue.intValue();
					}
				});
		
		addPredicate("ultra_greater_30",
				new Predicate(new Integer(30))
				{
					public boolean truthValue()
					{
						Integer compValue = (Integer)param1;
						return ultrasonic>compValue.intValue();
					}
				});
		
		addPredicate("ultra_greater_20",
				new Predicate(new Integer(20))
				{
					public boolean truthValue()
					{
						Integer compValue = (Integer)param1;
						return ultrasonic>compValue.intValue();
					}
				});
		
		
		addPredicate("ultra_greater_50",
				new Predicate(new Integer(50))
				{
					public boolean truthValue()
					{
						Integer compValue = (Integer)param1;
						return ultrasonic>compValue.intValue();
					}
				});
		

		// less
		
		addPredicate("ultra_less_40",
				new Predicate(new Integer(40))
				{
					public boolean truthValue()
					{
						Integer compValue = (Integer)param1;
						return ultrasonic<compValue.intValue();
					}
				});
		
		addPredicate("ultra_less_30",
				new Predicate(new Integer(30))
				{
					public boolean truthValue()
					{
						Integer compValue = (Integer)param1;
						return ultrasonic<compValue.intValue();
					}
				});
		
		addPredicate("ultra_less_20",
				new Predicate(new Integer(20))
				{
					public boolean truthValue()
					{
						Integer compValue = (Integer)param1;
						return ultrasonic<compValue.intValue();
					}
				});
		
		
		addPredicate("ultra_less_50",
				new Predicate(new Integer(50))
				{
					public boolean truthValue()
					{
						Integer compValue = (Integer)param1;
						return ultrasonic<compValue.intValue();
					}
				});
		
		addPredicate("left_lower_wheel_efficiency_1",
				new Predicate(new Integer(1))
				{
					public boolean truthValue()
					{
						Integer compValue = (Integer)param1;
						float leftSTD=leftEfficiencyStats.getSampleSTD();
						if (leftSTD>0)
						{
							return (leftEfficiencyStats.getSampleAverage()-leftMotorEfficiency)>compValue*leftSTD;
						}
						else
							return false;
						
					}
				});
		
		addPredicate("left_lower_wheel_efficiency_2",
				new Predicate(new Integer(2))
				{
					public boolean truthValue()
					{
						Integer compValue = (Integer)param1;
						float leftSTD=leftEfficiencyStats.getSampleSTD();
						if (leftSTD>0)
						{
							return (leftEfficiencyStats.getSampleAverage()-leftMotorEfficiency)>compValue*leftSTD;
						}
						else
							return false;
						
					}
				});
		
		addPredicate("left_higher_wheel_efficiency_1",
				new Predicate(new Integer(1))
				{
					public boolean truthValue()
					{
						Integer compValue = (Integer)param1;
						float leftSTD=leftEfficiencyStats.getSampleSTD();
						if (leftSTD>0)
						{
							return (leftMotorEfficiency-leftEfficiencyStats.getSampleAverage())>compValue*leftSTD;
						}
						else
							return false;
						
					}
				});
		
		addPredicate("left_higher_wheel_efficiency_2",
				new Predicate(new Integer(2))
				{
					public boolean truthValue()
					{
						Integer compValue = (Integer)param1;
						float leftSTD=leftEfficiencyStats.getSampleSTD();
						if (leftSTD>0)
						{
							return (leftMotorEfficiency-leftEfficiencyStats.getSampleAverage())>compValue*leftSTD;
						}
						else
							return false;
						
					}
				});
		
		// higher
		
		
		
		addPredicate("right_lower_wheel_efficiency_1",
				new Predicate(new Integer(1))
				{
					public boolean truthValue()
					{
						Integer compValue = (Integer)param1;
						float rightSTD=rightEfficiencyStats.getSampleSTD();
						if (rightSTD>0)
						{
							return (rightEfficiencyStats.getSampleAverage()-rightMotorEfficiency)>compValue*rightSTD;
						}
						else
							return false;
						
					}
				});
		
		addPredicate("right_lower_wheel_efficiency_2",
				new Predicate(new Integer(2))
				{
					public boolean truthValue()
					{
						Integer compValue = (Integer)param1;
						float rightSTD=rightEfficiencyStats.getSampleSTD();
						if (rightSTD>0)
						{
							return (rightEfficiencyStats.getSampleAverage()-rightMotorEfficiency)>compValue*rightSTD;
						}
						else
							return false;
						
					}
				});
		
		addPredicate("right_higher_wheel_efficiency_1",
				new Predicate(new Integer(1))
				{
					public boolean truthValue()
					{
						Integer compValue = (Integer)param1;
						float rightSTD=rightEfficiencyStats.getSampleSTD();
						if (rightSTD>0)
						{
							return (rightMotorEfficiency-rightEfficiencyStats.getSampleAverage())>compValue*rightSTD;
						}
						else
							return false;
						
					}
				});
		
		addPredicate("right_higher_wheel_efficiency_2",
				new Predicate(new Integer(2))
				{
					public boolean truthValue()
					{
						Integer compValue = (Integer)param1;
						float rightSTD=rightEfficiencyStats.getSampleSTD();
						if (rightSTD>0)
						{
							return (rightMotorEfficiency-rightEfficiencyStats.getSampleAverage())>compValue*rightSTD;
						}
						else
							return false;
						
					}
				});
		
		addPredicate("left_tach_1",
				new Predicate(new Integer(1))
				{
					public boolean truthValue()
					{
						Integer compValue = (Integer)param1;
						Integer baseTach = (Integer)param2;
						return compValue.intValue()*180<Math.abs(leftMotorTach-baseTach.intValue()); // approx 90 degree rotation
						
					}
				});
		
		addPredicate("left_tach_2",
				new Predicate(new Integer(2))
				{
					public boolean truthValue()
					{
						Integer compValue = (Integer)param1;
						Integer baseTach = (Integer)param2;
						return compValue.intValue()*180<Math.abs(leftMotorTach-baseTach.intValue());
						
					}
				});
		
		addPredicate("left_tach_3",
				new Predicate(new Integer(5))
				{
					public boolean truthValue()
					{
						Integer compValue = (Integer)param1;
						Integer baseTach = (Integer)param2;
						return compValue.intValue()*180<Math.abs(leftMotorTach-baseTach.intValue());
						
					}
				});
		
		
		addPredicate("left_tach_4",
				new Predicate(new Integer(10))
				{
					public boolean truthValue()
					{
						Integer compValue = (Integer)param1;
						Integer baseTach = (Integer)param2;
						return compValue.intValue()*180<Math.abs(leftMotorTach-baseTach.intValue());
						
					}
				});
		
		// right tachs
		addPredicate("right_tach_1",
				new Predicate(new Integer(1))
				{
					public boolean truthValue()
					{
						Integer compValue = (Integer)param1;
						Integer baseTach = (Integer)param2;
						return compValue.intValue()*180<Math.abs(rightMotorTach-baseTach.intValue()); // approx 90 degree rotation
						
					}
				});
		
		addPredicate("right_tach_2",
				new Predicate(new Integer(2))
				{
					public boolean truthValue()
					{
						Integer compValue = (Integer)param1;
						Integer baseTach = (Integer)param2;
						return compValue.intValue()*180<Math.abs(rightMotorTach-baseTach.intValue());
						
					}
				});
		
		addPredicate("right_tach_3",
				new Predicate(new Integer(5))
				{
					public boolean truthValue()
					{
						Integer compValue = (Integer)param1;
						Integer baseTach = (Integer)param2;
						return compValue.intValue()*180<Math.abs(rightMotorTach-baseTach.intValue());
						
					}
				});
		
		
		addPredicate("right_tach_4",
				new Predicate(new Integer(10))
				{
					public boolean truthValue()
					{
						Integer compValue = (Integer)param1;
						Integer baseTach = (Integer)param2;
						return compValue.intValue()*180<Math.abs(rightMotorTach-baseTach.intValue());
						
					}
				});
		
	}
	
	
	private void updatePredicates()
	{
		for (String predname:predicateMap.keySet())
		{
			if (predicateMap.get(predname).truthValue())
			{
				predicateValues.put(predname, "T");
			}
			else
				predicateValues.remove(predname);
		}
	}
	
	private void resetLeftTach()
	{
		
	}
	
	
	private ActionResponse executeFunction(String function, HashSet<String> parameters)
	{
		Hashtable<String,String> actionDefinition;
		actionDefinition=getActionDefinition(function);
		if (actionDefinition==null)
			return null;
		
		// Check if function still running
		
		if (actionExitP(function))
			return null;
		String newStep;
		
		// Check if current segment still runnable
		if (segmentExitP(function))
		{
			// TODO: Make this work for cyclical actions
			if (atMaxStepP(actionDefinition))
				return null;
			
			newStep=incrementStep(actionDefinition);
		}
		else
			newStep=extractCurrentStep(actionDefinition);
		
		Hashtable<String,String> segmentDef = getFunctionContextMap(function);
		addSegmentDefCondition(segmentDef,newStep);
		String newAction = getReferencedFunction(segmentDef);
		if (newAction==null) // This is a primitive action
		{
			return getPrimitiveResponse(function);
		}
		else
		{
			ActionResponse response=executeFunction(newAction,null);
		}
		
		
		
	}
	
	
	private String incrementStep(Hashtable<String,String> actionDefinition)
	{
		String currentStep = extractCurrentStep(actionDefinition);
		String newStep = ""+Integer.parseInt(currentStep)+1;
		controller.changeValueInConsistentWorlds(actionDefinition, step_filter_key, newStep);
		return newStep;
	}
	
	private boolean actionExitP(String functionName)
	{
		Hashtable<String,String> actionQuery = getFunctionContextMap(functionName);
		updateQueryWithPredicates(actionQuery);
		addExitCondition(actionQuery);
		String[] stopWorlds = controller.getAllConsistentWorlds(actionQuery);
		return (stopWorlds!=null&&stopWorlds.length>0);
	}
	
	private boolean segmentExitP(String functionName)
	{
		Hashtable<String,String> actionQuery = getFunctionContextMap(functionName);
		updateQueryWithPredicates(actionQuery);
		addSegmentExitCondition(actionQuery);
		String[] stopWorlds = controller.getAllConsistentWorlds(actionQuery);
		return (stopWorlds!=null&&stopWorlds.length>0);
	}
	
	
	private boolean atMaxStepP(Hashtable<String,String> mappedWorld)
	{
		String stepNumber=extractCurrentStep(mappedWorld);
		String maxSteps = extractMaxSteps(mappedWorld);
		return Integer.parseInt(stepNumber)==Integer.parseInt(maxSteps);
	}
	
	private String getReferencedFunction(Hashtable<String,String> segmentDef)
	{
		Hashtable<String,String> world=getFirstConsistentWorld(segmentDef);
		return extractReferenceAction(world);
	}
	
	private Hashtable<String,String> getFirstConsistentWorld(Hashtable<String,String> map)
	{
		String[] stepWorlds = controller.getAllConsistentWorlds(map);
		Hashtable<String,String> mappedValues = StandardTools.parseToKeyValueMap(stepWorlds[0]);
		if (mappedValues.containsKey(step_filter_key))
			return mappedValues;
		else
			return null;
	}
	
	
	

	private String extractReferenceAction(Hashtable<String,String> mappedValues)
	{
		if (mappedValues.containsKey(action_ref_filter_key))
			return mappedValues.get(action_ref_filter_key);
		else
			return null;
	}
	
	
	
	private String extractCurrentStep(Hashtable<String,String> mappedValues)
	{
		if (mappedValues.containsKey(step_filter_key))
			return mappedValues.get(step_filter_key);
		else
			return null;
	}
	
	private String getMaxSteps(String functionName)
	{
		Hashtable<String,String> actionQuery = getFunctionContextMap(functionName);
		addActionDefinition(actionQuery);
		String[] stepWorlds = controller.getAllConsistentWorlds(actionQuery);
		Hashtable<String,String> mappedValues = StandardTools.parseToKeyValueMap(stepWorlds[0]);
		if (mappedValues.containsKey(total_steps_filter_key))
			return mappedValues.get(total_steps_filter_key);
		else
			return null;
	}
	
	
	private String extractMaxSteps(Hashtable<String,String> mappedValues)
	{
		if (mappedValues.containsKey(total_steps_filter_key))
			return mappedValues.get(total_steps_filter_key);
		else
			return null;
	}
	
	/**
	 * Creates a hashtable<String,String> that already filters for a given function name
	 * @param functionName
	 * @return
	 */
	private Hashtable<String,String> getFunctionContextMap(String functionName)
	{
		Hashtable<String,String> map = new Hashtable<String, String>();
		map.put(function_filter_key, functionName);
		return map;
	}
	
	private void addExitCondition(Hashtable<String,String> map)
	{
		map.put(kvExitConditionFilter.GetKey(), kvExitConditionFilter.GetValue());
	}
	
	private void addSegmentExitCondition(Hashtable<String,String> map)
	{
		map.put(kvSegmentExitFilterPair.GetKey(), kvSegmentExitFilterPair.GetValue());
	}
	
	private void addActionDefinition(Hashtable<String,String> map)
	{
		map.put(kvActionDefFilter.GetKey(), kvActionDefFilter.GetValue());
	}
	
	private void addSegmentDefCondition(Hashtable<String,String> map)
	{
		map.put(kvSegmentDefPair.GetKey(), kvSegmentDefPair.GetValue());
	}

	private void addSegmentDefCondition(Hashtable<String,String> map, String index)
	{
		map.put(kvSegmentDefPair.GetKey(), kvSegmentDefPair.GetValue());
		map.put(step_filter_key, index);
	}
	
	/**
	 * Adds the sensor predicate evidence pairs to the hashtable
	 * @param queryMap
	 */
	private void updateQueryWithPredicates(Hashtable<String,String> queryMap)
	{
		for (String key:predicateValues.keySet())
		{
			queryMap.put(key, predicateValues.get(key));
		}
	}
	
	
	/** Returns a Hashtable representation of the basic action definition structure
	 * 
	 * @param functionName
	 * @return null if there are no worlds consistent with this function name
	 */
	private Hashtable<String,String> getActionDefinition(String functionName)
	{
		Hashtable<String,String> queryArgs = getFunctionContextMap(functionName);
		addActionDefinition(queryArgs);
		String[] worlds=controller.getAllConsistentWorlds(queryArgs);
		if ((worlds!=null)&&(worlds.length>0))
		{
			return StandardTools.parseToKeyValueMap(worlds[0]);
		}
		else
			return null;
	}
	
	private void addPredicate(String name, Predicate value)
	{
		predicateMap.put(name, value);
	}
	
	
	// TODO: Must fill this out
	private ActionResponse getPrimitiveResponse(String function)
	{
		return null;
	}


}
